{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import sys\n",
    "sys.path.insert(0,'../')\n",
    "sys.path.insert(0,'../../../data_analysis/')\n",
    "from simulate_door_slam import *\n",
    "from trajectory import *\n",
    "from utils import *\n",
    "from local_plot import *\n",
    "\n",
    "def read_frame_timestamps(g2o_path, nodes):\n",
    "    frame_id_to_stamp = {}\n",
    "    for i in nodes:\n",
    "        with open(f\"{g2o_path}/frame_timestamp_{i}.txt\", \"r\") as f:\n",
    "            for line in f:\n",
    "                frame_id, stamp = line.split()\n",
    "                frame_id_to_stamp[int(frame_id)] = float(stamp)\n",
    "    return frame_id_to_stamp\n",
    "\n",
    "def process_output(g2o_path, nodes):\n",
    "    poses_pgo, t0 = read_paths(g2o_path, nodes, prefix=\"pgo_\")\n",
    "    paths_gt, _ = read_paths(g2o_path + \"/../\", nodes, prefix=\"groundtruth_\", t0=t0)\n",
    "    align_paths(poses_pgo, paths_gt, align_by_first=True, align_with_minize=True)\n",
    "    display(plot_fused_err(nodes, poses_pgo, paths_gt, show=False))\n",
    "    # plot_fused(nodes, poses_pgo, paths_gt)\n",
    "    dte = 550\n",
    "    display(relative_pose_err(nodes, poses_pgo, paths_gt, dte=dte))\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Note\n",
    "We need to start ROS Core Manually for this file"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def evaluate_pgo(g2o_path, g2o_input, max_steps=100, eta_k=1.45, rho_frame_T=0.39, \n",
    "            rho_frame_theta=1.556, simulate_delay_ms=0.0, verify_accuaracy=False, agent_num=5, dgs_max_steps=50, \n",
    "            max_solving_time=10.0, rho_rot_mat=0.09):\n",
    "    pg = PoseGraph(g2o_input)\n",
    "    ignore_infor = False\n",
    "    print(f\"Initial cost: {pg.evaluate_cost():.1f} edges {len(pg.edges)}\")\n",
    "    if verify_accuaracy:\n",
    "        frame_id_to_stamp = read_frame_timestamps(g2o_path, range(agent_num))\n",
    "    output_path=g2o_path + \"/d2pgo-rot-inited/\"\n",
    "    pgo_optimized, ret = call_d2pgo_opti(g2o_folder=g2o_input, output_folder=output_path, enable_rot_init=True, \n",
    "        max_steps=max_steps, agent_num=agent_num, ignore_infor=ignore_infor, eta_k=eta_k, rho_frame_theta=rho_frame_theta, \n",
    "        rho_frame_T=rho_frame_T, rho_rot_mat=rho_rot_mat, simulate_delay_ms=simulate_delay_ms, max_solving_time=max_solving_time)\n",
    "    if verify_accuaracy:\n",
    "        pgo_optimized.write_to_csv(output_path, frame_id_to_stamp)\n",
    "        process_output(output_path, [0, 1, 2, 3, 4])\n",
    "    print(\"Avg Traj len: \", pgo_optimized.evaluate_trajectory_length())\n",
    "    print(f\"D2PGO RotInit cost:         {pgo_optimized.evaluate_cost():.5f} solve_time {ret['max_solve_time']:.1f} ms iters {ret['mean_iters']}\")\n",
    "\n",
    "    output_path=g2o_path + \"/d2pgo-no-rot-init/\"\n",
    "    pgo_optimized, ret  = call_d2pgo_opti(g2o_folder=g2o_input, output_folder=output_path, enable_rot_init=False, \n",
    "        max_steps=max_steps, agent_num=agent_num, ignore_infor=ignore_infor, eta_k=eta_k, rho_frame_theta=rho_frame_theta, \n",
    "        rho_frame_T=rho_frame_T, rho_rot_mat=rho_rot_mat, simulate_delay_ms=simulate_delay_ms, max_solving_time=max_solving_time)\n",
    "    print(f\"D2PGO without RotInit cost: {pgo_optimized.evaluate_cost():.5f} solve_time {ret['max_solve_time']:.1f} ms iters {ret['mean_iters']}\")\n",
    "    if verify_accuaracy:\n",
    "        pgo_optimized.write_to_csv(output_path, frame_id_to_stamp)\n",
    "        process_output(output_path, [0, 1, 2, 3, 4])\n",
    "    # output_path = g2o_path + \"/DGS/\"\n",
    "    # iterations, min_time, max_time, initial, final, util_rate, total_optim = call_DGS_solver(g2o_input, agent_num=agent_num, \n",
    "    #     rthresh=1e-4, pthresh=1e-4, maxIter=dgs_max_steps, between_noise=\"true\")\n",
    "    # pgo_optimized = copy.copy(pg)\n",
    "    # pgo_optimized.read_g2o_single(g2o_input+\"/fullGraph_optimized.g2o\", update_only=True)\n",
    "    # total_time = max_time + iterations*simulate_delay_ms #Assume communication time is 100ms\n",
    "    # print(f\"DGS cost: {pgo_optimized.evaluate_cost():.1f} max_time: {max_time} iter {iterations} total_time {total_time} DGS cost initial {initial} final: {final} \")\n",
    "    # if verify_accuaracy:\n",
    "    #     pgo_optimized.write_to_csv(output_path, frame_id_to_stamp)\n",
    "    #     process_output(output_path, [0, 1, 2, 3, 4])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "g2o_path=\"/home/xuhao/data/d2slam/pgo/tum_corr_5/\"\n",
    "g2o_input = g2o_path + \"/input/\"\n",
    "max_steps = 1000\n",
    "eta_k=1.5101010101010102\n",
    "rho_frame_T=0.4526572657265727\n",
    "rho_frame_theta=2.868058805880588\n",
    "rho_rot_mat =0.0918787878787879\n",
    "simulate_delay_ms=50.0\n",
    "max_solving_time =20.0\n",
    "\n",
    "evaluate_pgo(g2o_path, g2o_input, max_steps, eta_k, rho_frame_T, rho_frame_theta, \n",
    "                simulate_delay_ms, max_solving_time=max_solving_time, rho_rot_mat=rho_rot_mat, dgs_max_steps=40)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "g2o_path=\"/home/xuhao/data/d2slam/pgo/parking-garage/\"\n",
    "g2o_input = g2o_path + \"/input/\"\n",
    "max_steps = 500\n",
    "evaluate_pgo(g2o_path, g2o_input, max_steps, eta_k, rho_frame_T, rho_frame_theta, \n",
    "                simulate_delay_ms, max_solving_time=max_solving_time, rho_rot_mat=rho_rot_mat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "g2o_path=\"/home/xuhao/data/d2slam/pgo/cubicle/\"\n",
    "g2o_input = g2o_path + \"/input/\"\n",
    "evaluate_pgo(g2o_path, g2o_input, max_steps, eta_k, rho_frame_T, rho_frame_theta, \n",
    "                simulate_delay_ms, max_solving_time=max_solving_time, rho_rot_mat=rho_rot_mat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "g2o_path=\"/home/xuhao/data/d2slam/pgo/OmniLong5Yaw/\"\n",
    "g2o_input = g2o_path + \"/input/\"\n",
    "max_solving_time = 20\n",
    "evaluate_pgo(g2o_path, g2o_input, max_steps, eta_k, rho_frame_T, rho_frame_theta, \n",
    "                simulate_delay_ms, max_solving_time=max_solving_time, rho_rot_mat=rho_rot_mat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "g2o_path=\"/home/xuhao/data/d2slam/pgo/ri_realsense_walkaround_2022_10/\"\n",
    "g2o_input = g2o_path + \"/input/\"\n",
    "max_solving_time = 20\n",
    "max_steps = 1000\n",
    "eta_k=1.5101010101010102\n",
    "rho_frame_T=0.4526572657265727\n",
    "rho_frame_theta=2.868058805880588\n",
    "rho_rot_mat =0.0918787878787879\n",
    "simulate_delay_ms=50.0\n",
    "max_solving_time=3.0\n",
    "\n",
    "evaluate_pgo(g2o_path, g2o_input, max_steps, eta_k, rho_frame_T, rho_frame_theta, simulate_delay_ms, \n",
    "                    max_solving_time=max_solving_time, rho_rot_mat=rho_rot_mat, agent_num=3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
